//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <core.h>
#include <_hal.h>
#include <_context.h>
#include <init.h>
#include <entry.h>

//
// ThreadHContext
//
INLINE void ThreadHContext::SaveFPU()
{
    if (m_uFlags & ThreadHContextFlag_FPUUsed) {
        ASM("fnsave %0;"
            :"=m"(m_fpuContext));
        ASM("fwait;");

        m_uFlags &= ~ThreadHContextFlag_FPUUsed;

        Stts();
    }
}

void ThreadHContext::RestoreFPU()
{
    uint32_t uFlags = SaveFlagsAndCli();
    Clts();

    if (m_uFlags & ThreadHContextFlag_FPUInited) {
        ASM("frstor %0;"
            : :"m"(m_fpuContext));
    }
    else {
        ASM("fninit;");
        m_uFlags |= ThreadHContextFlag_FPUInited;
    }

    m_uFlags |= ThreadHContextFlag_FPUUsed;
    RestoreIF(uFlags);
}

void ThreadHContext::Save()
{
    SaveFPU();
    m_vaPageFaultAddress = GetPageFaultAddress();
}

void ThreadHContext::Restore()
{
    SetPageFaultAddress(m_vaPageFaultAddress);
}

#define INIT_EFLAG  EFLAG_IF

EXTERN virtaddr_t InitThread0Stack(Thread *pThread)
{
    virtaddr_t *pvaKernelStackPtr =
        (virtaddr_t *)pThread->m_vaKernelStack - 1;
    *pvaKernelStackPtr = (virtaddr_t)&Thread0Startup;

    return (virtaddr_t)pvaKernelStackPtr;
}

//
// The layout of new system thread stack is:
//
// thread.m_vaKernelStack ->      |                      |
//                                +----------------------+
//                                |     pvParameter      |
//                                +----------------------+
//                                |    threadroutine     |
//                                +----------------------+
//                                |     threadentry      |
//                                +----------------------+
// thread.m_vaKernelStackPtr ->   |    return address    | SystemThreadStartup
//                                +----------------------+
//

EXTERN virtaddr_t InitSystemThreadStack(
    Thread *pThread, threadentry_t threadentry,
    threadroutine_t threadroutine, void *pvParameter)
{
    virtaddr_t *pvaKernelStackPtr = (virtaddr_t *)pThread->m_vaKernelStack - 1;
    *pvaKernelStackPtr = pvParameter;

    pvaKernelStackPtr--;
    *pvaKernelStackPtr = (virtaddr_t)threadroutine;

    pvaKernelStackPtr--;
    *pvaKernelStackPtr = (virtaddr_t)threadentry;

    pvaKernelStackPtr--;
    *pvaKernelStackPtr = (virtaddr_t)&SystemThreadStartup;

    return (virtaddr_t)pvaKernelStackPtr;
}

//
// The layout of new thread stack is:
//
// thread.m_vaKernelStack ->      |                    |
//                                +--------------------+
//                                |                    |
//                                |  InterruptContext  |
//                                |                    |
//                                +--------------------+
//                                |    pvParameter     |
//                                +--------------------+
//                                |   threadroutine    |
//                                +--------------------+
// thread.m_vaKernelStackPtr ->   |   return address   | ThreadStartup
//                                +--------------------+
//

EXTERN virtaddr_t InitThreadStack(
    Thread *pThread, threadroutine_t threadroutine, void *pvParameter)
{
    InterruptContext *pContext =
        (InterruptContext *)pThread->m_vaKernelStack - 1;

    virtaddr_t *pvaKernelStackPtr = (virtaddr_t *)pContext - 1;
    *pvaKernelStackPtr = pvParameter;

    pvaKernelStackPtr--;
    *pvaKernelStackPtr = (virtaddr_t)threadroutine;

    pvaKernelStackPtr--;
    *pvaKernelStackPtr = (virtaddr_t)&ThreadStartup;

    return (virtaddr_t)pvaKernelStackPtr;
}

EXTERN void SetThreadUserEntry(
    Thread *pThread, virtaddr_t vaUserStack, virtaddr_t vaUserEntry)
{
    InterruptContext *pContext =
        (InterruptContext *)pThread->m_vaKernelStack - 1;

    pContext->ds     = USER_DATA_SELECTOR;
    pContext->es     = USER_DATA_SELECTOR;
    pContext->eip    = (uint32_t)vaUserEntry;
    pContext->cs     = USER_CODE_SELECTOR;
    pContext->eflags = INIT_EFLAG;
    pContext->_esp   = (uint32_t)vaUserStack;
    pContext->_ss    = USER_DATA_SELECTOR;
    pContext->ebp    = NULL;
}

//
// ProcessHContext
//
void ProcessHContext::ActivateDebugRegisters()
{
#ifndef KCONFIG_GLOBAL_BREAKPOINT
    if (0 == m_debugRegisters[7]) {
        SetDR7(0);
    }
    else {
        SetDR0(m_debugRegisters[0]);
        SetDR1(m_debugRegisters[1]);
        SetDR2(m_debugRegisters[2]);
        SetDR3(m_debugRegisters[3]);
        SetDR6(m_debugRegisters[6]);
        SetDR7(m_debugRegisters[7]);
    }
#endif // KCONFIG_GLOBAL_BREAKPOINT
}

void ProcessHContext::GetDebugRegisters(DebugContext *pContext)
{
    pContext->dr0 = m_debugRegisters[0];
    pContext->dr1 = m_debugRegisters[1];
    pContext->dr2 = m_debugRegisters[2];
    pContext->dr3 = m_debugRegisters[3];
    pContext->dr6 = m_debugRegisters[6];
    pContext->dr7 = m_debugRegisters[7];
}

void ProcessHContext::SetDebugRegisters(DebugContext *pContext)
{
    m_debugRegisters[0] = pContext->dr0;
    m_debugRegisters[1] = pContext->dr1;
    m_debugRegisters[2] = pContext->dr2;
    m_debugRegisters[3] = pContext->dr3;
    m_debugRegisters[6] = pContext->dr6;
    m_debugRegisters[7] = pContext->dr7;
}

ECode ProcessHContext::SetBreakPoint(
    uint_t uNo, BreakPointType type, void *pvAddress, uint_t uLength)
{
#ifdef KCONFIG_GLOBAL_BREAKPOINT
    return E_NOT_IMPLEMENTED;
#else
    if (3 < uNo) return E_NOT_IMPLEMENTED;

    SetDR7(m_debugRegisters[7] & ~DR7_G(uNo));

    uint32_t dr7 = 0;

    switch (type) {
        case BreakPointType_Exec:
            dr7 |= DR7_RW(uNo, DR7_RWFIELD_EO);
            break;
        case BreakPointType_Write:
            dr7 |= DR7_RW(uNo, DR7_RWFIELD_WO);
            break;
        case BreakPointType_ReadWrite:
            dr7 |= DR7_RW(uNo, DR7_RWFIELD_RW);
            break;
        default:
            return E_INVALID_ARGUMENT;
    }

    switch (uLength) {
        case 1:
            dr7 |= DR7_LEN(uNo, DR7_LENFIELD_1BYTE);
            break;
        case 2:
            dr7 |= DR7_LEN(uNo, DR7_LENFIELD_2BYTES);
            break;
        case 4:
            dr7 |= DR7_LEN(uNo, DR7_LENFIELD_4BYTES);
            break;
        default:
            return E_INVALID_ARGUMENT;
    }

    switch (uNo) {
        case 0:
            m_debugRegisters[0] = (uint32_t)pvAddress;
            SetDR0((uint32_t)pvAddress);
            break;
        case 1:
            m_debugRegisters[1] = (uint32_t)pvAddress;
            SetDR1((uint32_t)pvAddress);
            break;
        case 2:
            m_debugRegisters[2] = (uint32_t)pvAddress;
            SetDR2((uint32_t)pvAddress);
            break;
        case 3:
            m_debugRegisters[3] = (uint32_t)pvAddress;
            SetDR3((uint32_t)pvAddress);
            break;
        default:
            assert0();
    }

    m_debugRegisters[7] |= dr7 | DR7_G(uNo) | DR7_GE;
    SetDR7(m_debugRegisters[7]);

    return NOERROR;
#endif // KCONFIG_GLOBAL_BREAKPOINT
}

#ifdef KCONFIG_GLOBAL_BREAKPOINT
EXTERN ECode SetGlobalBreakPoint(
    uint_t uNo, BreakPointType type, void *pvAddress, uint_t uLength)
{
    STATIC uint32_t s_debugRegisters[NUMBEROF_DEBUGREGISTERS];

    if (3 < uNo) return E_NOT_IMPLEMENTED;

    SetDR7(s_debugRegisters[7] & ~DR7_G(uNo));

    uint32_t dr7 = 0;

    switch (type) {
        case BreakPointType_Exec:
            dr7 |= DR7_RW(uNo, DR7_RWFIELD_EO);
            break;
        case BreakPointType_Write:
            dr7 |= DR7_RW(uNo, DR7_RWFIELD_WO);
            break;
        case BreakPointType_ReadWrite:
            dr7 |= DR7_RW(uNo, DR7_RWFIELD_RW);
            break;
        default:
            return E_INVALID_ARGUMENT;
    }

    switch (uLength) {
        case 1:
            dr7 |= DR7_LEN(uNo, DR7_LENFIELD_1BYTE);
            break;
        case 2:
            dr7 |= DR7_LEN(uNo, DR7_LENFIELD_2BYTES);
            break;
        case 4:
            dr7 |= DR7_LEN(uNo, DR7_LENFIELD_4BYTES);
            break;
        default:
            return E_INVALID_ARGUMENT;
    }

    switch (uNo) {
        case 0:
            s_debugRegisters[0] = (uint32_t)pvAddress;
            SetDR0((uint32_t)pvAddress);
            break;
        case 1:
            s_debugRegisters[1] = (uint32_t)pvAddress;
            SetDR1((uint32_t)pvAddress);
            break;
        case 2:
            s_debugRegisters[2] = (uint32_t)pvAddress;
            SetDR2((uint32_t)pvAddress);
            break;
        case 3:
            s_debugRegisters[3] = (uint32_t)pvAddress;
            SetDR3((uint32_t)pvAddress);
            break;
        default:
            assert0();
    }

    s_debugRegisters[7] |= dr7 | DR7_G(uNo) | DR7_GE;
    SetDR7(s_debugRegisters[7]);

    return NOERROR;
}
#endif // KCONFIG_GLOBAL_BREAKPOINT
